
#include "filter.h"
#include "ALL_DEFINE.h"


#define N 20      //滤波缓存数组大小
#define M_PI_F 3.1416f

/*******************************************************************************
*函  数 ：float FindPos(float*a,int low,int high)
*功  能 ：确定一个元素位序
*参  数 ：a  数组首地址
*         low数组最小下标
*         high数组最大下标
*返回值 ：返回元素的位序low
*备  注 : 无
*******************************************************************************/
float FindPos(float*a,int low,int high)
{
    float val = a[low];                      //选定一个要确定值val确定位置
    while(low<high)
    {
        while(low<high && a[high]>=val)
             high--;                       //如果右边的数大于VAL下标往前移
             a[low] = a[high];             //当右边的值小于VAL则复值给A[low]

        while(low<high && a[low]<=val)
             low++;                        //如果左边的数小于VAL下标往后移
             a[high] = a[low];             //当左边的值大于VAL则复值给右边a[high]
    }
    a[low] = val;//
    return low;
}

/*******************************************************************************
*函  数 ：void QuiteSort(float* a,int low,int high)
*功  能 ：快速排序
*参  数 ：a  数组首地址
*         low数组最小下标
*         high数组最大下标
*返回值 ：无
*备  注 : 无
*******************************************************************************/
void QuiteSort(float* a,int low,int high)
{
 int pos;
 if(low<high)
 {
	 pos = FindPos(a,low,high); //排序一个位置
	 QuiteSort(a,low,pos-1);    //递归调用
	 QuiteSort(a,pos+1,high);
 }
}

 /*******************************************************************************
*函  数 ：float  SortAver_Filter(float value)
*功  能 ：去最值平均值滤波一组数据
*参  数 ：value 采样的数据
*返回值 ：返回滤波后的数据
 *备  注 : 无
*******************************************************************************/
void  SortAver_Filter(float value,float *filter,uint8_t n)
{
	static float buf[N] = {0.0};
	static uint8_t cnt =0,flag = 1;
	float temp=0;
	uint8_t i=0;
	buf[cnt++] = value;
	if(cnt<n && flag) 
		return;  //数组填不满不计算	
	else flag=0; 
    QuiteSort(buf,0,n-1);
	for(i=1;i<n-1;i++)
	 {
		temp += buf[i];
	 }

	 if(cnt>=n) cnt = 0;

	 *filter = temp/(n-2);
}

void  QSortAver_Filter(float value,float *filter,uint8_t n)
{
	static float bufqqq[N] = {0.0};
	static uint8_t cnt =0,flag = 1;
	float temp=0;
	uint8_t i=0;
	bufqqq[cnt++] = value;
	if(cnt<n && flag) 
		return;  //数组填不满不计算	
	else flag=0; 
    QuiteSort(bufqqq,0,n-1);
	for(i=1;i<n-1;i++)
	 {
		temp += bufqqq[i];
	 }

	 if(cnt>=n) cnt = 0;

	 *filter = temp/(n-2);
}

void  ASortAver_Filter(float value,float *filter,uint8_t n)
{
	static float bufaaa[N] = {0.0};
	static uint8_t cnt =0,flag = 1;
	float temp=0;
	uint8_t i=0;
	bufaaa[cnt++] = value;
	if(cnt<n && flag) 
		return;  //数组填不满不计算	
	else flag=0; 
    QuiteSort(bufaaa,0,n-1);
	for(i=1;i<n-1;i++)
	 {
		temp += bufaaa[i];
	 }

	 if(cnt>=n) cnt = 0;

	 *filter = temp/(n-2);
}
/*******************************************************************************
*函  数 ：void  SortAver_FilterXYZ(INT16_XYZ *acc,FLOAT_XYZ *Acc_filt,uint8_t n)
*功  能 ：去最值平均值滤波三组数据
*参  数 ：*acc 要滤波数据地址
*         *Acc_filt 滤波后数据地址
*返回值 ：返回滤波后的数据
*备  注 : 无
*******************************************************************************/
void  SortAver_FilterXYZ(INT16_XYZ *acc,FLOAT_XYZ *Acc_filt,uint8_t n)
{
	static float bufx[N],bufy[N],bufz[N];
	static uint8_t cnt =0,flag = 1;
	float temp1=0,temp2=0,temp3=0;
	uint8_t i;
	bufx[cnt] = acc->X;
	bufy[cnt] = acc->Y;
	bufz[cnt] = acc->Z;
	cnt++;      //这个的位置必须在赋值语句后，否则bufx[0]不会被赋值
	if(cnt<n && flag) 
		return;   //数组填不满不计算
	else
		flag = 0;
	
    QuiteSort(bufx,0,n-1);
	QuiteSort(bufy,0,n-1);
	QuiteSort(bufz,0,n-1);
	for(i=1;i<n-1;i++)
	 {
		temp1 += bufx[i];
		temp2 += bufy[i];
		temp3 += bufz[i];
	 }

	 if(cnt>=n) cnt = 0;
	 Acc_filt->X  = temp1/(n-2);
	 Acc_filt->Y  = temp2/(n-2);
	 Acc_filt->Z  = temp3/(n-2);
}

/*******************************************************************************
*函  数 ：void Aver_FilterXYZ6(INT16_XYZ *acc,INT16_XYZ *gry,FLOAT_XYZ *Acc_filt,
                              FLOAT_XYZ *Gry_filt,uint8_t n)
*功  能 ：滑动窗口滤波六组数据
*参  数 ：*acc ,*gry 要滤波数据地址
*         *Acc_filt,*Gry_filt 滤波后数据地址
*返回值 ：返回滤波后的数据
*备  注 : 无
*******************************************************************************/
void Aver_FilterXYZ6(INT16_XYZ *acc,INT16_XYZ *gry,FLOAT_XYZ *Acc_filt,FLOAT_XYZ *Gry_filt,uint8_t n)
{
  static float bufax[N],bufay[N],bufaz[N],bufgx[N],bufgy[N],bufgz[N];
	static uint8_t cnt =0,flag = 1;
	float temp1=0,temp2=0,temp3=0,temp4=0,temp5=0,temp6=0;
	uint8_t i;
	bufax[cnt] = acc->X;
	bufay[cnt] = acc->Y;
	bufaz[cnt] = acc->Z;
	bufgx[cnt] = gry->X;
	bufgy[cnt] = gry->Y;
	bufgz[cnt] = gry->Z;
	cnt++;      //这个的位置必须在赋值语句后，否则bufax[0]不会被赋值
	if(cnt<n && flag) 
		return;   //数组填不满不计算
	else
		flag = 0;
	for(i=0;i<n;i++)
	{
		temp1 += bufax[i];
		temp2 += bufay[i];
		temp3 += bufaz[i];
		
		temp4 += bufgx[i];
		temp5 += bufgy[i];
		temp6 += bufgz[i];
	}
	 if(cnt>=n) cnt = 0;
	 Acc_filt->X  = temp1/n;
	 Acc_filt->Y  = temp2/n;
	 Acc_filt->Z  = temp3/n;
	
	 Gry_filt->X  = temp4/n;
	 Gry_filt->Y  = temp5/n;
	 Gry_filt->Z  = temp6/n;
	
}

/*******************************************************************************
*函  数 ：void Aver_FilterXYZ(INT16_XYZ *acc,FLOAT_XYZ *Acc_filt,uint8_t n)
*功  能 ：滑动窗口滤波三组数据
*参  数 ：*acc  要滤波数据地址
*         *Acc_filt 滤波后数据地址
*返回值 ：返回滤波后的数据
*备  注 : 无
*******************************************************************************/
void Aver_FilterXYZ(INT16_XYZ *acc,FLOAT_XYZ *Acc_filt,uint8_t n)
{
  static int32_t bufax[N],bufay[N],bufaz[N];
	static uint8_t cnt =0,flag = 1;
	int32_t temp1=0,temp2=0,temp3=0,i;
	bufax[cnt] = acc->X;
	bufay[cnt] = acc->Y;
	bufaz[cnt] = acc->Z;
	cnt++;      //这个的位置必须在赋值语句后，否则bufax[0]不会被赋值
	if(cnt<n && flag) 
		return;   //数组填不满不计算
	else
		flag = 0;
	for(i=0;i<n;i++)
	{
		temp1 += bufax[i];
		temp2 += bufay[i];
		temp3 += bufaz[i];
	}
	 if(cnt>=n)  cnt = 0;
	 Acc_filt->X  = temp1/n;
	 Acc_filt->Y  = temp2/n;
	 Acc_filt->Z  = temp3/n;
     
}

/*******************************************************************************
*函  数 ：void Aver_Filter(float data,float *filt_data,uint8_t n
*功  能 ：滑动窗口滤波一组数据
*参  数 ：data  要滤波数据
*         *filt_data 滤波后数据地址
*返回值 ：返回滤波后的数据
*备  注 : 无
*******************************************************************************/
void Aver_Filter(float data,float *filt_data,uint8_t n)
{
  static float buf[N];
	static uint8_t cnt =0,flag = 1;
	float temp=0;
	uint8_t i;
	buf[cnt] = data;
	cnt++;
	if(cnt<n && flag) 
		return;   //数组填不满不计算
	else
		flag = 0;
	
	for(i=0;i<n;i++)
	{
		temp += buf[i];
	}
	if(cnt>=n) cnt = 0;
	 *filt_data = temp/n;
}

 /*******************************************************************************
*函  数 ：float  SortAver_Filter(float value)
*功  能 ：去最值平均值滤波一组数据
*参  数 ：value 采样的数据
*返回值 ：返回滤波后的数据
 *备  注 : 无
*******************************************************************************/
void INTSortAver_Filter(uint16_t value,uint16_t *filter,uint8_t n)
{
	static float buf[N] = {0.0};
	static uint8_t cnt =0,flag = 1;
	uint16_t temp=0;
	uint8_t i=0;
	buf[cnt++] = value;
	if(cnt<n && flag) 
		return;  //数组填不满不计算	
	else 
		flag=0; 
    QuiteSort(buf,0,n-1);
	for(i=1;i<n-1;i++)
	 {
		temp += buf[i];
	 }

	 if(cnt>=n) cnt = 0;

	 *filter = temp/(n-2);
}

void Com_Gry(float angle_m, float gyro_m,float *angle_R)  
{

	float K1 =0.01f;//0.0246
	*angle_R = K1 * angle_m + (1-K1) * (*angle_R + gyro_m * 0.2866f);  //0.2866 = 1/200*57.3
}

void Gyro_MagCom(float angle_m, float gyro_m,float *angle_R,float dt)  
{

	float K1 =0.02f; //
	*angle_R = K1 * angle_m + (1-K1) * (*angle_R + gyro_m * dt * RadtoDeg);  //0.2866 = 1/200*57.3
}





void RC_Filter_(float kout_coe,float in,float *out)
{
	*out=kout_coe*(*out)+(1-kout_coe)*in;
}

void RC_OneFilter_(float kout_coe,float in,float *out)
{
	*out=(1-kout_coe)*(*out)+kout_coe*in;
}


void Com_LF(float angle_m, float gyro_m,float *angle_R,float dt)  
{

	float K1 =0.05f;  //0.0246
	*angle_R = K1 * angle_m + (1-K1) * (*angle_R + gyro_m *dt);  
}




void RC_CI(float angle_m, float gyro_m,float *angle_R,float coe,float dt)  
{
	*angle_R = coe * angle_m + (1-coe) * (*angle_R + gyro_m *dt);  
	
}

